//
//  Plane.hpp
//  ProjektMagisterski
//
//  Created by Marcin Stramowski on 11.02.2017.
//
//
#pragma once
#ifndef Plane_h
#define Plane_h
#ifdef __GNUC__
// Avoid tons of warnings with root code
#pragma GCC system_header
#endif

#include <Point3D.h>
#include <PosOrient.h>
#include <PointCloud.h>
#include <point_cloud/MapManager.hpp>

using namespace Eigen;
using namespace std;

class Plane 
{    
public:
    Plane();
    Plane(vector<float> coefficients, const PointCloud::Ptr& cloud);
    Plane(const Vector3d& _normal, const Vector3d& _mean, const PointCloud::Ptr& cloud);
    Plane(Vector3d _normalVec, Vector3d _mean, vector<Point3D> _points);
    Plane(const Vector3d &point1, const Vector3d &point2, const Vector3d &point3, Vector3d _meanPoint);
    // 直接给出平面的参数
    Plane(double D, const Vector3d &planeNormalVec);
    Plane(Eigen::Quaterniond quaternion);

    float getPercentOfWall() {return percent_of_wall;}
    double getA() const { return A; }
    double getB() const { return B; }
    double getC() const { return C; }
    double getD() const { return D; }
    long getId() const { return id; }
    long getFrameId() const {return frame_id;}
    int getLabel() const{ return label; }
    const Vector3d &getMeanPoint() const { return meanPoint; }
    const vector<Point3D> &getPoints() const { return points; }
    const PointCloud::Ptr& getPointCloud() const { return cloud; }
    Point3DCloud getPoints3D() const { return points3D; }
    Vector3d getPlaneNormalVec() const { return planeNormalVec; }
    Point3D getCentralPoint() const {return points[(points.size() - 1) / 2];} 
    OctreeManager* getOctreeManager() const {return octree; }

    const pair<PointType, PointType>& getAABB() const { return aabb;}
    Plane getPlaneSeenFromGlobalCamera(PosOrient &pos);
    Plane getPlaneSeenFromGlobalCamera(Eigen::Matrix4d &posMatrix);
    double getDistanceFromPoint(Eigen::Vector3d point) const;
    double getDistanceFromPoint(const Vector3d &point);
    double getMeanDistanceFromPointCloud(const vector<Point3D> &points);
    double getAngleBetweenTwoPlanes(const Plane &plane) const;
    double getDistanceFromPlane(Plane &plane);
    float getMeanDistanceBetweenTwoPlanes(const Plane& plane);
    unsigned int getNumberOfPoints() const;

    bool getPositiveFromPoint(Vector3d point);
    bool isValid() const { return valid; }
    bool isWasMatched() const { return wasMatched; }
    bool isMatchWith(Plane &plane);
    bool isEqualTo(Plane &plane);
    bool isCloseTo(const Plane& plane);

    void setAABB(const pair<PointType, PointType>& _aabb) { aabb = _aabb;}
    void setFrameId(long _id) { frame_id = _id;}
    void setLabel(int _label) { label = _label; }
    void setPercentOfWall(float value) { percent_of_wall = value; }
    void setWasMatched(bool _wasMatched) { wasMatched = _wasMatched; } 
    void setPoints(const vector<Point3D> &_points) {points = _points; }
    void setPoints(PointCloud::Ptr &cloud);
    void setMeanPoint(Eigen::Vector3d _meanPoint) { meanPoint = _meanPoint; }
    void setId(long _id) { id = _id; }
    
    void transformParas(const Eigen::Matrix4d& t);
    void transform(const Eigen::Matrix4d& t);
    void clearPoints();
    void clearAllPointsExceptSome();
    void insertPoints(const vector<Point3D>& points);
    void insertPointCloud(const PointCloud::Ptr& points);
    void updatePlaneParameters(Plane &plane);
    void transformPlaneToGlobal(Eigen::Matrix4d &posMatrix);
    void transformMeanToGlobal(Eigen::Matrix4d &t);
    void transformParametersToGlobal(Eigen::Matrix4d &t);
    void transformPointsToGlobal(Eigen::Matrix4d &t);
    void transformPointsToGlobal(PosOrient &posOrient);
    void computeNormalDirection();
    void computeNormalVecDirection();
    void computeAABB();
    Vector3d computeMean(const PointCloud::Ptr &points);
    // Vector3d computeMean(const vector<Point3D> &pointsVector);
    void computeMean();
    
    void print(){ 
        std::cout << "A: " << A << " B: " << B << " C: " << C << " D: " << D << std::endl;
    }
    
    friend std::ostream &operator<<(std::ostream &stream, const Plane &plane) {
        stream << plane.getA() << " " << plane.getB() << " " << plane.getC() << " " << plane.getD();
        return stream;
    }

    bool operator == (const Plane &plane) const {
        return(this->getA() == plane.getA() && this->getB() == plane.getB()
               && this->getC() == plane.getC() && this->getD() == plane.getD());
    }
    
    void operator=(Plane plane)
    {
        this->A = plane.getA();
        this->B = plane.getB();
        this->C = plane.getC();
        this->D = plane.getD();
        //this->points = plane.getPoints();
        this->cloud = plane.getPointCloud();
        this->octree = plane.getOctreeManager();
        this->planeNormalVec = plane.getPlaneNormalVec();
        this->meanPoint = plane.getMeanPoint();
        this->setLabel(plane.getLabel());
        this->setFrameId(plane.getFrameId());
        this->setPercentOfWall(plane.getPercentOfWall());
        this->setId(plane.getId());
    }

    // void operator=(Plane &plane){
    //     this->A = plane.getA();
    //     this->B = plane.getB();
    //     this->C = plane.getC();
    //     this->D = plane.getD();
    //     this->points = plane.getPoints();
    //     this->planeNormalVec = plane.getPlaneNormalVec();
    //     this->meanPoint = plane.getMeanPoint();
    // }

    
private:
    int planeId = 0;
    int label;
    long id = -1;
    long frame_id = -1;
    double A, B, D, C;
    float percent_of_wall = 0.5;
    vector<Point3D> points;
    OctreeManager* octree; // 用来保持点云不至于太密
    PointCloud::Ptr cloud;

    Point3DCloud points3D;
    Vector3d planeNormalVec;
    bool valid = false;
    void computePlaneEquation(const Vector3d &point1, const Vector3d &point2, const Vector3d &point3);
    bool wasMatched = false;
    Vector3d meanPoint = Vector3d(0, 0, 0);
    pair<PointType, PointType> aabb;
    //PointType min_point_AABB;
    //PointType max_point_AABB;
};



#endif /* Plane_h */
